#include "PX4CtrlFSM.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rc_test_node");
    ros::NodeHandle nh;
    
    ros::Rate loop_rate(30);  // 设置循环频率为10Hz
    // init class PX4CtrlFSM
    PX4CtrlFSM px4_ctrl_rc(nh);
    px4_ctrl_rc.loadParameters(nh);

    ros::Subscriber rc_in_sub =
        nh.subscribe<mavros_msgs::RCIn>("/mavros/rc/in",
                                         10,
                                         boost::bind(&PX4CtrlFSM::rc_in_cb, &px4_ctrl_rc, _1));
    ros::Subscriber uav_state_sub =
        nh.subscribe<mavros_msgs::State>("/mavros/state",
                                         10,
                                         boost::bind(&PX4CtrlFSM::uav_state_cb, &px4_ctrl_rc, _1));
    // init bool 只有在第一次进入case运行
    bool init_takeoff = false;
    bool init_landing = false;
    bool init_hover = false;
    bool init_dock = false;

    // Variable to track whether the drone has successfully landed
    bool landed = true;
    bool possible_landing = false;
    // wait for FCU connection
    while (ros::ok() && !px4_ctrl_rc.uav_cur_state.connected)
    {
        ros::spinOnce();
        ROS_INFO_STREAM("wait for FCU connection");
        loop_rate.sleep();
    }
    ROS_INFO_STREAM("\033[33m FCU Connected!! \033[0m");

    while (ros::ok())
    {
        ros::spinOnce();
        if (px4_ctrl_rc.uav_cur_state.mode == "OFFBOARD")
        {
            ROS_INFO_STREAM_ONCE("\033[33m In OFFBOARD Mode \033[0m");

            // switch modes: default start mode is MODE4: IDLE
            ROS_WARN("flight_mode: %s", px4_ctrl_rc.flightModeToString(px4_ctrl_rc.flight_mode).c_str());
            ROS_WARN("px4_ctrl_rc.rc_in_feedback = %d",px4_ctrl_rc.rc_in_feedback);
            // ================== MODE1: AUTO_TAKOFF --- condition : last_mode = IDLE && rc_in_takeoff
            if (px4_ctrl_rc.rc_data.takeoff && px4_ctrl_rc.flight_mode == PX4CtrlFSM::IDLE) {
                px4_ctrl_rc.flight_mode = PX4CtrlFSM::TAKEOFF;
                init_hover = false;
                init_landing = false;
                init_dock = false;

                landed = false;
                possible_landing = false;
                px4_ctrl_rc.rc_feed = {0.0, 0.0, 0.0};
                px4_ctrl_rc.landing_process1_flag = false;


            // ============ MODE2: AUTO_HOVER
            } else if (px4_ctrl_rc.rc_data.hover && !px4_ctrl_rc.rc_data.docking 
                        && px4_ctrl_rc.flight_mode != PX4CtrlFSM::IDLE
                        && px4_ctrl_rc.flight_mode != PX4CtrlFSM::HOVER && !landed) {
                px4_ctrl_rc.flight_mode = PX4CtrlFSM::HOVER;
                init_dock = false;
                init_landing = false;
                possible_landing = false;
                ROS_WARN("------------ RC RESET ------------");
                px4_ctrl_rc.rc_feed = {0.0, 0.0, 0.0};
                px4_ctrl_rc.landing_process1_flag = false;


            // ============ MODE3: AUTO_DOCKING
            } else if (px4_ctrl_rc.rc_data.docking && px4_ctrl_rc.flight_mode != PX4CtrlFSM::IDLE
                        && px4_ctrl_rc.flight_mode == PX4CtrlFSM::HOVER && !landed) {
                px4_ctrl_rc.flight_mode = PX4CtrlFSM::DOCKING;
                init_hover = false;
                init_landing = false;
                possible_landing = false;
                ROS_WARN("------------ RC RESET ------------");
                px4_ctrl_rc.rc_feed = {0.0, 0.0, 0.0};
                px4_ctrl_rc.landing_process1_flag = false;


            // ============ MODE3=4: AUTO_LANDING --- condition : last_mode = not IDLE && rc_in_landing
            } else if (px4_ctrl_rc.rc_data.landing && px4_ctrl_rc.flight_mode != PX4CtrlFSM::IDLE 
                        && px4_ctrl_rc.flight_mode != PX4CtrlFSM::LANDING && !landed) {
                px4_ctrl_rc.flight_mode = PX4CtrlFSM::LANDING;
                init_hover = false;
                init_takeoff = false;
                init_dock = false; 

                ROS_WARN("------------ RC RESET ------------");
                px4_ctrl_rc.rc_feed = {0.0, 0.0, 0.0};


            // ================== MODE4: IDLE
            } else if ((px4_ctrl_rc.rc_data.landing || px4_ctrl_rc.rc_data.hover) && landed) {
                px4_ctrl_rc.flight_mode = PX4CtrlFSM::IDLE;
            }

            // Based on the current flight mode, execute corresponding logic
            switch (px4_ctrl_rc.flight_mode) {
                // =============================== TAKEOFF CASE =========================== //
                case PX4CtrlFSM::TAKEOFF:{
                    ROS_INFO("[*******TAKEOFF******]");

                    // ============ TAKEOFF STEP1 : init - 在一次周期中 LANDING之后 往复起飞时重置
                    // get initial pose uav_ini_pose (x0,y0,z0) 
                    for (int i = 10; !init_takeoff && i > 0; --i)
                    {
                        ROS_INFO("[Initial takeoff]");
                        ros::spinOnce();
                        loop_rate.sleep();
                        
                    }
                    init_takeoff = true;

                    ROS_INFO("=========== RC Feedback ===========");
                    ROS_INFO("Pitch: %d, Roll: %d, Throttle: %d", 
                            px4_ctrl_rc.rc_in_pitch_feedback, px4_ctrl_rc.rc_in_roll_feedback, px4_ctrl_rc.rc_in_thro_feedback);

                        ROS_INFO("=========== Joystick Feedback ===========");
                        ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl_rc.rc_feed.x, px4_ctrl_rc.rc_feed.y, px4_ctrl_rc.rc_feed.z);
                        ROS_INFO("rc_rateX: %.3f, rc_rateY: %.3f, rc_rateZ: %.3f", 
                                px4_ctrl_rc.current_params.rc_rate.x, px4_ctrl_rc.current_params.rc_rate.y, px4_ctrl_rc.current_params.rc_rate.z);
                        ROS_INFO("rc_rangMIN: %.3f, rc_rangMAX: %.3f", 
                                px4_ctrl_rc.current_params.rc_range.min_xy, px4_ctrl_rc.current_params.rc_range.max_xy);
                    }
                    ros::spinOnce();
                    loop_rate.sleep();
                    break;

                // =============================== HOVER CASE =========================== //
                case PX4CtrlFSM::HOVER:{
                    ROS_INFO("[*******HOVER******]");

                    // ============ HOVER STEP1 : init - 切入hover重置
                    // get hover pose offb_hoverini_pose (xi,yi,zi) 
                    for (int i = 10; !init_hover && i > 0; --i)
                    {
                        ROS_INFO("[Initial Hovering]");
                        ros::spinOnce();
                        loop_rate.sleep();
                        
                    }
                    init_hover = true;

                    // js input will change the target 
                    ROS_INFO("=========== RC Feedback ===========");
                    ROS_INFO("Pitch: %d, Roll: %d, Throttle: %d", 
                            px4_ctrl_rc.rc_in_pitch_feedback, px4_ctrl_rc.rc_in_roll_feedback, px4_ctrl_rc.rc_in_thro_feedback);

                        ROS_INFO("=========== Joystick Feedback ===========");
                        ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl_rc.rc_feed.x, px4_ctrl_rc.rc_feed.y, px4_ctrl_rc.rc_feed.z);
                        ROS_INFO("rc_rateX: %.3f, rc_rateY: %.3f, rc_rateZ: %.3f", 
                                px4_ctrl_rc.current_params.rc_rate.x, px4_ctrl_rc.current_params.rc_rate.y, px4_ctrl_rc.current_params.rc_rate.z);
                        ROS_INFO("rc_rangMIN: %.3f, rc_rangMAX: %.3f", 
                                px4_ctrl_rc.current_params.rc_range.min_xy, px4_ctrl_rc.current_params.rc_range.max_xy);
                    ros::spinOnce();
                    loop_rate.sleep();
                    break;
                }

                // =============================== DOCKING CASE =========================== //
                case PX4CtrlFSM::DOCKING:{
                    ROS_INFO("[*******DOCKING******]");

                    // ============ DOCKING STEP1 : init - 切入Docking重置
                    // get Docking pose offb_dockini_pose (xi,yi,zi) 
                    for (int i = 10; !init_dock && i > 0; --i)
                    {
                        ROS_INFO("[Initial Docking]");
                        ros::spinOnce();
                        loop_rate.sleep();
                        
                    }
                    init_dock = true;

                    // js input will change the target 
                    ROS_INFO("=========== RC Feedback ===========");
                    ROS_INFO("Pitch: %d, Roll: %d, Throttle: %d", 
                            px4_ctrl_rc.rc_in_pitch_feedback, px4_ctrl_rc.rc_in_roll_feedback, px4_ctrl_rc.rc_in_thro_feedback);

                        ROS_INFO("=========== Joystick Feedback ===========");
                        ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl_rc.rc_feed.x, px4_ctrl_rc.rc_feed.y, px4_ctrl_rc.rc_feed.z);
                        ROS_INFO("rc_rateX: %.3f, rc_rateY: %.3f, rc_rateZ: %.3f", 
                                px4_ctrl_rc.current_params.rc_rate.x, px4_ctrl_rc.current_params.rc_rate.y, px4_ctrl_rc.current_params.rc_rate.z);
                        ROS_INFO("rc_rangMIN: %.3f, rc_rangMAX: %.3f", 
                                px4_ctrl_rc.current_params.rc_range.min_xy, px4_ctrl_rc.current_params.rc_range.max_xy);
                    ros::spinOnce();
                    loop_rate.sleep();
                    break;
                }


                // =============================== LANDING CASE =========================== //
                case PX4CtrlFSM::LANDING:{
                    ROS_INFO("[*******LANDING******]");

                    // ============ LANDING STEP1 : init - 在相信人操控的前提下,此时在当前位置悬停,并获取降落位置
                    for (int i = 10; !init_landing && i > 0; --i) 
                    {
                        ROS_INFO("[Initial Landing]");
                        ros::spinOnce();
                        loop_rate.sleep();

                    }
                    init_landing = true;
                    
                    // ============ LANDING STEP2 : Landing process 1 - 先保证xy并下降一定高度 可以动rc
                    while (!px4_ctrl_rc.landing_process1_flag)  //rc thro not the min
                    {
                        // 如果 px4_ctrl_rc.rc_data.landing 变为 false，立即退出循环
                        if (!px4_ctrl_rc.rc_data.landing)
                        {
                            ROS_WARN_THROTTLE(1, "Landing mode switched!");
                            possible_landing = true;
                            break;
                        }
                        ROS_INFO("[Landing process 1]");
                        ROS_INFO("=========== RC Feedback ===========");
                        ROS_INFO("Pitch: %d, Roll: %d, Throttle: %d", 
                                px4_ctrl_rc.rc_in_pitch_feedback, px4_ctrl_rc.rc_in_roll_feedback, px4_ctrl_rc.rc_in_thro_feedback);

                        ROS_INFO("=========== Joystick Feedback ===========");
                        ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl_rc.rc_feed.x, px4_ctrl_rc.rc_feed.y, px4_ctrl_rc.rc_feed.z);
                        ROS_INFO("rc_rateX: %.3f, rc_rateY: %.3f, rc_rateZ: %.3f", 
                                px4_ctrl_rc.current_params.rc_rate.x, px4_ctrl_rc.current_params.rc_rate.y, px4_ctrl_rc.current_params.rc_rate.z);
                        ROS_INFO("rc_rangMIN: %.3f, rc_rangMAX: %.3f", 
                                px4_ctrl_rc.current_params.rc_range.min_xy, px4_ctrl_rc.current_params.rc_range.max_xy);

                        ros::spinOnce();
                        loop_rate.sleep();
                        
                    }
                    ROS_INFO_STREAM("\033[33m =============== Landing process 1 done =============== \033[0m");

                    // ============ LANDING STEP2 : Landing process 2 - z 下降 可以动rc
                    while (!possible_landing) {

                        // 如果 px4_ctrl_rc.rc_data.landing 变为 false，立即退出循环
                        if (!px4_ctrl_rc.rc_data.landing)
                        {
                            ROS_WARN_THROTTLE(1, "Landing mode switched!");
                            landed = true;
                            break;
                        }

                        ROS_INFO("[Landing process 2]");
                        ROS_INFO("=========== RC Feedback ===========");
                        ROS_INFO("Pitch: %d, Roll: %d, Throttle: %d", 
                                px4_ctrl_rc.rc_in_pitch_feedback, px4_ctrl_rc.rc_in_roll_feedback, px4_ctrl_rc.rc_in_thro_feedback);

                        ROS_INFO("=========== Joystick Feedback ===========");
                        ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl_rc.rc_feed.x, px4_ctrl_rc.rc_feed.y, px4_ctrl_rc.rc_feed.z);
                        ROS_INFO("rc_rateX: %.3f, rc_rateY: %.3f, rc_rateZ: %.3f", 
                                px4_ctrl_rc.current_params.rc_rate.x, px4_ctrl_rc.current_params.rc_rate.y, px4_ctrl_rc.current_params.rc_rate.z);
                        ROS_INFO("rc_rangMIN: %.3f, rc_rangMAX: %.3f", 
                                px4_ctrl_rc.current_params.rc_range.min_xy, px4_ctrl_rc.current_params.rc_range.max_xy);

                        ros::spinOnce();
                        loop_rate.sleep();
                    }

                    // landed = true;

                    }
                    break;
                case PX4CtrlFSM::IDLE:{
                    // IDLE mode：Thrust = 0
                        ROS_INFO("[*******IDLE******]");
                        ROS_INFO("=========== RC Feedback ===========");
                        ROS_INFO("Pitch: %d, Roll: %d, Throttle: %d", 
                                px4_ctrl_rc.rc_in_pitch_feedback, px4_ctrl_rc.rc_in_roll_feedback, px4_ctrl_rc.rc_in_thro_feedback);

                        ROS_INFO("=========== Joystick Feedback ===========");
                        ROS_INFO("rc_feedX: %.3f, rc_feedY: %.3f, rc_feedZ: %.3f", px4_ctrl_rc.rc_feed.x, px4_ctrl_rc.rc_feed.y, px4_ctrl_rc.rc_feed.z);
                        ROS_INFO("rc_rateX: %.3f, rc_rateY: %.3f, rc_rateZ: %.3f", 
                                px4_ctrl_rc.current_params.rc_rate.x, px4_ctrl_rc.current_params.rc_rate.y, px4_ctrl_rc.current_params.rc_rate.z);
                        ROS_INFO("rc_rangMIN: %.3f, rc_rangMAX: %.3f", 
                                px4_ctrl_rc.current_params.rc_range.min_xy, px4_ctrl_rc.current_params.rc_range.max_xy);
                        ros::spinOnce();
                        loop_rate.sleep();
                    }
                    break;
                
            }
        }
        else
        {
            ROS_WARN("waiting for Offboard cmd");
            // ROS_WARN_THROTTLE(1, "waiting for Offboard cmd");
        }

        loop_rate.sleep();
    }

    return 0;
}